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1. Introduction 

A precise specification of the trajectory of the end effector is a prerequisite towards successful 
application of a manipulator to many tasks. Arc welding, spray painting, conveyor belt tracking, 
and glueing are some tasks which require specification of both the spatial and temporal aspects 
of a trajectory. In the most general case, not only the Cartesian position and velocity of the end 
effector, or hand, must be specified, but also the Cartesian accelerations. 

The transformation of a Cartesian trajectory of the hand into the corresponding joint angle 
trajectory of the manipulator, the so-called inverse kinematics problem, has been studied primarily 
in the context of positions and velocities. Reasonably efficient algorithms have been developed 
for these transformations (Paul 1981, Featherstone 1983), yet little attention has been paid to the 
solution of the inverse kinematic accelerations. 

In this paper, we present an efficient algorithm for the calculation of the inverse kinematic 
accelerations for a 6 degree-of-freedom (dof) manipulator with a spherical wrist, based on a 
technique developed by Featherstone (1983) for inverse kinematic positions and velocities. In 
addition, we show that the inverse kinematic calculations work synergistically with the inverse 
dynamic calculations, because the extended Featherstone method yields kinematic parameters 
needed in the backward recursion steps of die Newton-Euler dynamics formulation (Luh, Walker, 
and Paul 1980a). We note that consistency argues diat dynamics be particularized as well 
to spherical-wrist arms, resulting in considerable computational savings. Lastly, we examine 
simplifications in the dynamics computation due to simply-structured inertial parameters as well 
as to simply-structured kinematic parameters. 

1.1. Inverse Kinematic Positions 

A benign kinematic structure is a characteristic of most manipulators. Whereas kinematicians 
might choose to treat arbitrary linkages, manipulators are usually designed to satisfy simplifying 
kinematic criteria: 

(i) there is the kinematic equivalent of a spherical wrist, and 

(ii) neighboring joint axes are oriented at 0" or 90° relative to each other. 

Pieper (1968) originally showed that a wrist with three intersecting axes of rotation, which is 
kinematically equivalent to a spherical wrist, is one of the configurations that leads to an analytic 
inverse kinematic position calculation. The spherical wrist allows a decomposition of the 6-dof 
inverse kinematics computation into two 3-dof kinematic computations, through a separation of 
the orientation specification from the position specification. Most 6-dof manipulators are designed 
with spherical wrists, making diis the most important case. 

If the manipulator does not satisfy one of Pieper's criteria, then in general the inverse 
kinematic positions must be found by time-consuming numerical approximation and convergence 
methods. This renders real-time control of a Cartesian trajectory infeasible (but see the discussion 
of resolved rate control below). Some manipulators diat were originally designed without spherical 
wrists have been redesigned for this reason. Such manipulators might have been intended to be 
programmed by a human operator manually guiding die manipulator to desired positions with a 
teach box, so that the human operator in effect substituted for an inverse kinematics solution. As 
the manipulator's programming was increasingly automated with higher-level computer control, 
die non-spherical wrist was found to be an insurmountable stumbling block to automatic real-time 
generation of Cartesian trajectories. 
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Figure 1. Cartesian position and orientation specification for a reference point on the hand of a manipulator. 

1.2. Inverse Kinematic Velocities 

Shortly after Pieper's work, Whitney (1969, 1972) proposed the Resolved Motion Rate Control 
Method, which computes the inverse kinematic velocities while avoiding the computation of the 
inverse kinematic positions. Whereas an analytic inverse kinematic position calculation is critically 
dependent on a benign manipulator configuration, the inverse kinematic velocities can be easily 
computed for any arbitrary 6-dof manipulator. If 9 = (0i,...,0 6 ) is the vector of joint angles 
and x = (ii,...,i 6 ) is the vector specifying the position (11,3:2,2:3) = {x,y, z) and orientation 
(x4,x 5! x 6 ) = {0 x> 6y,0 z ) of the reference point on die hand (Fig. 1), then the forward kinematic 
f\ positions are straightforwardly given by trie transformation x = f(0). The forward kinematic 

velocities are then given by 

x = Si (1) 

where the elements J i} of the Jacobian J are dfi/dx 3 with f = (fu---,h). The E-2 manipulator 
that Whitney presented in his papers did not have a spherical wrist, and Whitney proposed that 
the trajectory be differentially approximated by solving the inverse kinematic velocities through 
inversion of (1) to avoid the intractible inverse' kinematic position calculation: 

g = J- a x (2) 

Due to the prohibitive computational cost of solving (2) through 6x6 matrix inversion (Table 1), 
Whitney suggested an interpolation scheme based on precomputcd inverse Jacobians evaluated at 
a few positions. 

It should be noted that the inverse Jacobian itself is often not of interest, but only the 
joint rates. Thus Gaussian elimination could have been used instead of matrix inversion, giving a 
substantial computational savings (Table 1); the numbers include evaluation of J. 

More recently, Paul (1981) proposed a method particularized to the Stanford manipulator 
which is somewhat more efficient than Gaussian elimination (Table 1); the numbers given 
are slightly larger than stated in (Paul 1981). Paul's method involves manipulation of 4 x 4 
transformation matrices, and takes implicit advantage of the spherical-wrist configuration of the 
Stanford manipulator. 
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Method 


Multiplications 


Additions 


6x6 matrix inversion 


287 


193 


Gaussian elimination 


141 


98 


Paul (1981) 


94 


55 


Featherstone (1983) 


36 


20 



Table 1. Computational complexity of various methods of computing the inverse kinematic velocities 
for a 6-dof manipulator, in terms of the total number of multiplications and additions required, for the 
Stanford arm. 

Featherstone (1983) takes more explicit advantage of the spherical-wrist configuration of 
manipulators to propose a highly efficient method for computation of the inverse kinematic 
velocities. The method is composed of four steps. 

(i) Find the linear velocity of the wrist from the hand linear and angular velocity. 

(ii) Find the first three joint rates from the wrist linear velocity. 

(iii) Find the angular velocity of the hand relative to the forearm. 

(iv) Find the last three joint rates from the relative angular velocity of the hand. 

Featherstone's method cannot be directly compared to Paul's method because it was developed 
for a different manipulator and because the Cartesian velocity specification was different. We have 
reworked Featherstone's method for die Stanford manipulator with Paul's efT G Cartesian velocity 
specification, where the wrist linear velocity and the time derivative of the hand orientation matrix 
are directly given. Thus the first step in Featherstone's method is unnecessary for purposes of 
the comparison. Table 1 shows that Featherstone's method is almost 3 times more efficient than 
Paul's method. Featherstone's method works so well because he most directly takes advantage of 
the spherical wrist kinematics of robots. This is particularly important in computing die wrist joint 
rates, where Paul's method requires 75 multiplications to Featherstone's 22. 

1.3. Inverse Kinematic Accelerations 

As mentioned earlier, a solution to the inverse kinematic accelerations is required in the most 
general transformation between Cartesian space and joint space. For purposes of control, the joint 
accelerations are required for input into the inverse dynamics as well as for recent control law 
formulations in terms of hand acceleration. For the inverse dynamics (Johnson 1983), 

I = N- 1 (Hg + c(0,g)) (3) 

where 

1 is the vector of joint torques, 

N is a matrix which reflects the recursive structure of the dynamic equations, 

H is the generalized inertia matrix, and 

c(#, 9) is the vector of centripetal and Coriolis torques. 

Several schemes have also been proposed recently that close the feedback loop around the 
hand. Adopting some of the results and notation from (Johnson 1983), these schemes include: 

1. Resolved acceleration (Luh, Walker, and Paul 1980b): 
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Method 


Multiplications 


Additions 


6x6 matrix inversion 


394 


260 


Gaussian elimination 


213 


162 


Wrist partitioning 


78 


57 



Table 2. Computational complexity of various methods of computing the inverse kinematic accelerations for 
a 6-dof manipulator. 



x = x d + Kj x d + K 2 (x — \ d ) (4) 

2. Nonlinear control (Freund 1982): 

x = Ax d + K lX + K 2 x (5) 

3. Cartesian impedance control (Hogan and Cotter 1982): 

x = M- X (f e - Ki(x) - K 2 (x) + K a (x„)) (6) 

where 

x is the derived hand acceleration, 

x and x are the measured hand velocity and position (as determined from the joint 
angle measurements), 

/•"N, x d , x d , and x d are the desired or planned hand acceleration, velocity, and position, 

A and Ki are velocity gain matrices, 

K 2 is a position gain matrix, 

f e is a vector of external forces, and 

M = diag(m, m, m, h,h, h) is a matrix which relates accelerations to generalized forces 
through the mass m of the hand and the principal inertias h,h, h. 

As can be seen, the exact formulations of the feedback laws (4)-(6) differ somewhat. Walker's 
method is the only one requiring a planned hand acceleration x d . In Hogan's method, the velocity 
and position gains Ki,K 2 are considered as arbitrary functions rather than just as matrices. In 
addition, Hogan also includes the external force f e and gives the hand attributes of mass and 
inertia by the matrix multiplication M — 1 . 

Once the hand acceleration x has been derived, whether by a trajectory planner or by a 
hand-based control law, the joint accelerations can be found through differentiation of (1). 

x = J0 + jg (7) 

0=J _1 (x-J2) (8) 

As in the inverse kinematic velocity computation, solution of the inverse kinematic accelerations 
by matrix inversion (8) is very costly (Table 2). Again, since the inverse Jacobian itself is not 
of interest, the joint accelerations are better found by solving (7) through Gaussian elimination 
(Table 2). Until now, there had been no better method than Gaussian elimination, but it can be 
seen that the wrist-partitioning algorithm developed later is substantialy more efficient. 
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Figure 2. (a) First three joints of the rotary manipulator, (b) Spherical wrist of the rotary manipulator. 

The proposals of Walker, Freund, and Hogan, however, seem to require the matrix inversion 
method. The hand acceleration is incorporated into the inverse dynamics computation (3) by 
substituting for the joint accelerations (8). 



r = N-^HJ-^x - jfl) + c(0,< 



(9) 



If they had intended Gaussian elimination, then die inverse kinematics and dynamics should 
have been expressed in the forms (3) and (7). Because of the apparent difficulty of solving the 
inverse kinematic accelerations, Khatib (1980) proposed a hand-based feedback law which involves 
resolving hand forces through the inverse Jacobian rather than resolving the hand accelerations. 



2. Rotary Manipulator 

We now proceed to derive an efficient formulation of the inverse kinematic accelerations. To 
demonstrate the method, we use a rotary manipulator without offsets. Other manipulator types 
are readily adapted to Featherstone's method, although die details primarily in step 2 and slightly 
in step 3 must be reworked; the other computations remain the same if the manipulator has a 
spherical wrist. 

The first three joints are shown in Figure 2a and the last three in Figure 2b. The joint axes 
for Uiis manipulator are derived from the Denavit-Hartenberg (1955) specification. The rotation 
axis Zi corresponds to joint angle i+1 between links i and t -j- 1. The internal coordinate system 
for link i is completed by defining x, from the cross product Z;_i x z t , which also locates the 
origin, and y» = z t x x,. Neighboring coordinate systems are related by three parameters: 

a; is the distance between /;_! and z, measured along x it 

S{ is the distance between Xj_i and x, measured along %i—\, and 
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O, Si 


<*t 


1 


Si 


tt/2 


2 


o 2 





3 





— tt/2 


4 


s 4 


tt/2 


5 





-tt/2 


6 









Table 3. Denavit-Hartenberg parameters for the rotary manipulator of Figure 2. 



f*S 



on is the angle between Zj_i and z, measured in a righthand sense about x,. 

The values for the three parameters above for the rotary arm of Figure 2 are given in Table 3. 

The directions for z, and x, are chosen so that when 9 t = 0, then x,_j and X; are parallel and 
pointing in the same direction. For the rotary manipulator, this desideratum is straightforwardly 
satisfied for every coordinate sytem except 3 and 5. For coordinate system 3, we desire that the 
rotation axis Z3 point at the wrist. This means that x 3 must point in the opposite direction from 
Z2 X z 3 , which is reflected by a 3 = — tt/2, and that the zero position for die elbow joint is at a 
right angle pointing upward (Figure 2a). Similarly, because it is desired to point z 5 towards the 
tip of the hand, x 4 must point in the opposite direction from z 4 x z 5 , and a 5 = — irj2 (Figure 
2b). 

The rotation matrix A t which transforms points expressed in link % coordinates to link i — 1 
coordinates is: 



A, 



c9 t — s^jCa, s0iS<Xi 
sOi cQiCcti — cOiSoti 
scti cdi 



(10) 



where we have used the abbreviations s0 — sin0 and c6 
the six joint transformation matrices. 



cos 9. For convenience, we list below 



Ai 



A 4 = 



'c9 1 

S0! 

. 1 


s9i 

— C0i 




A 2 = 


'c9 2 
s9 2 
. 


— s9 2 ' 
c9 2 
1 . 




A 3 = 


"C0 3 -S0 3 
S0 3 C0 3 

.0-1 


~c6 4 

504 

- 1 


S0 4 
— C0 4 




A 5 = 


C0 5 
S0 5 
- 


-S0 5 
C0 5 

-1 




A 6 = 




"c0 6 -s0 6 

506 C0 6 

. 1 



(11) 



With these transformation matrices, points on any one link may be referred to the coordinate 
system of any other link. We will denote with a left superscript which coordinate system a vector is 
referred to. Then 'v is a vector referred to the ith coordinate system, and *~ *v = A, 'v is this same 
vector referred to the i — 1st coordinate system. These transformation matrices can be chained 
together to refer non-adjacent links i and j (where j > i) by defining 'W,- = A i+1 A i+2 ---A 
where 'v = 
coordinate 0, 



■3< 



'Wy J v. By convention, the left superscript is omitted when referring to the base 
e.g., v = °v. 



Define the following vectors, which will be useful later: 
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elbow 



shoulder /l- — " 




base 
Figure 3. Vector definitions between various coordinate origins of the rotary manipulator. 

p, is the vector from coordinate origin to coordinate origin i, and 

p* is the vector from coordinate origin t — 1 to coordinate origin »'. 

Due to the coincidence of coordinate origins 2-3 and of 4-6, many of these vectors are not distinct. 
Thus p,_i = p t and p* — for i = 3, 5, 6. Furthermore, Pi = pj. 

3. Specification of the Cartesian Trajectory 

In applying Featherstone's method, it is necessary to have available the angular velocity and 
acceleration vectors w 6 , w 6 of the hand as well as a time history of the (x, y, z) Cartesian position of 
some point on the hand. Yet some of the more common Cartesian trajectory planning algorithms, 
such as those built around straight-line, constant-velocity segments (Paul 1981, Taylor 1979), yield 
instead of the angular velocity and acceleration vectors a time-varying hand orientation matrix 
W 6 . In such a case, the angular velocity and acceleration of the hand can be derived from the 
orientation matrix. 

If p 7 is a vector from the base to the point on the tip of the hand used in trajectory planning, 
and p 7 is the internal vector from coordinate origin 6 to this same tip point (Figure 3), then for 
a spherical-wrist arm 



P7 =P4 + W 6 6 p 7 

p 7 =P4 + W 6 6 Pt 
Pt =P 4 + W 6 6 P ; 



(12) 
(13) 
(14) 



where the first two time derivatives of the rotation matrix W 6 must be available. In the vectorial 
representation, 



f>7 =P4 + W 6 X W 6 6 P 7 

P 7 =P 4 + w 6 X W 6 6 p; + w 6 X (Ma X W 6 6 p^) 



(15) 
(16) 
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Figure 4. (a) Projection of the wrist point onto the io — yo plane to find 6\ . (b) Reduction to a planar 
two-link manipulator for 6 2 and 6 3 by referring positions to link 1 coordinates. 

Equating the corresponding elements, 



ul 6 =W 6 WJ 

«L =W 6 Wj + \V 6 W 



IT 



(17) 
(18) 



where w 6 and w 6 are the matrix representations for the cross product by w 6 and w 6 respectively; 
that is to say, £> 6 v = w 6 x v for example. 

4. Inverse Kinematic Positions 

In applying Featherstone's method, it is necessary to have solved for the inverse kinematic positions 
before finding die inverse kinematic velocities, and to have solved for the inverse kinematic 
velocities before finding the inverse kinematic accelerations. For completeness of presentation and 
to show how intermediate results from one inverse kinematic level are used in the next, we begin 
with a rederivation of the inverse kinematic positions. 

Step I: Find the wrist position We presume that the orientation matrix W 6 for the hand and 
the position p 7 of the tip of the hand have been specified. Since 6 p^, the internal vector in link 
6 which extends from the coordinate 6 origin to the hand tip, is known, then the position of the 
wrist p 4 is given by 



P4 



p 7 -w 6 6 P ; 



(19) 



Step 2: Find the first three joint angles. Joint 1 is directly found from p 4 , since joints 2 and 
3 act in a plane that does not change the projection of the wrist onto the i » j/o plane (Figure 4a): 



Hollerbach and Sahar 



Wrist-Partitioned Inverse Kinematics 



^ m \ [ 



&i = tan" 



,PixJ 



(20) 



where the tan -1 function corresponds to the FORTRAN ATAN2 function, so that the above 
division is not actually carried out. We follow Paul (1981) in this practice because of the superior 
numerical conditioning with this inverse trigonometric function compared to others. A degeneracy 
occurs when the wrist lies on the z axis, where p 4j = p 4y = and Q x can assume any value. 
Since sQ x and c0 x are required below, they are best found with one additional transcendental 
function call. Defining r 2 — p\ x + p\ y , 



C0! 



P4x 



SO, — 



P4y 



(21) 
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The next two angles are easily found if p 4 is expressed in joint 1 coordinates, which reduces 
the problem to a planar two-link problem. Define 



P 4 — Pi == p 4 — SjZq 



When expressed in link 1 coordinates, 



A[p„ 



r 

Pwz 

L . 



(22) 



(23) 



where 1 p wz — because the wrist point p w lies in the zj/j/i plane. By the cosine rule (Figure 
4b), 



c0 3 is found next. 



'3 = 



2d 2 S4 



(24) 






(25) 
(26) 



Depending on -which quadrant is chosen for 3 , one obtains an elbow up or elbow down solution. 
The angle 6 2 is found by expressing 1 p w in terms of the joint axis vectors, 



a 2 x 2 -f- s 4 Z3 

a 2 c0 2 — 5 4 s(0 2 + #3)" 

a 2 s8 2 + Sic{9 2 +8$) 





(27) 
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Solving by simultaneous equations, 



a% + s 4 — 2a 2 (s 4 s0 3 ) 

a _ l Pwx-\- s9 2 {s i c9 3 ) 
a 2 — (s4S$ 3 ) 



(28) 
(29) 
(30) 



where the products in the parentheses are computed only once. 

Step 3: Find the hand orientation relative to the forearm. The forearm orientation is given by 
W 3 = AiA 2 A 3 , so that the hand orientation relative to the forearm, 3 W 6 = A 4 A 5 A 6 , is given by 



3 Wfi = wrw. 



(31) 



Because they yield intermediate results which are useful later, we present the partial matrix 
multiplications AjA 2 and (AiA 2 )A 3 . 



f*\ 



AiA 2 



(AiA 2 )A 3 



"c9iC$2 — C$iS$2 S$i 
S$iC$2 — S$iS$2 — c9\ 

s6 2 C$2 

c9 3 {ce 1 ce 2 ) — se s (cd 1 se 2 ) 

C03(S0!C0 2 ) — S0 3 (S^ 1 5^ 2 ) 

s{e 2 + 63) 



-s$i — S0 3 (c0ic02) — C0 3 (c0is0 2 ) 
c9 x —s9 3 (s9ic9 2 ) — c9 3 {s9is9 2 ) 
c{9 2 + 9 3 ) 



(32) 



(33) 



where the sine/cosine products in the parentheses are computed only once and the 31 and 33 
elements in (33) are computed from the expansion. Finally, we presume 3 W 6 has been computed 
from a knowledge of W 6 and 9 lt 9 2 ,9 3 , and that its elements are w i:j . 

Step 4: Find the last three joint angles. Joint angles 4 and 9 6 can be found from the elements 
of 3 W 6 , which are identified by some matrix manipulations. 



/"*S 



A4A5 = 



3 W 6 A[ 



'c9ic9 z — s0 4 — c9^s9 5 

S0 4 C# 5 C$i — s9is9 5 

■ s9 5 c9 5 

w u c9 6 —wi 2 s9 6 w n s9 6 +w i2 c9 6 w n 

W 2 1C9 6 — W 22 s9 G W 2 iS9 6 4" W 22 C$ 6 ™23 

w 31 c9 6 — w 32 s9 6 w S is$ 6 + w 32 c9 6 w 33 



Since A 4 A 5 = 3 W 6 A-T, we find by selecting elements 13 and 23 that 



10 



(34) 



(35) 
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04 



tan" 



-if — w 23 *\ 
\—W 13 ) 



(36) 



Because of the multiplication factor s9 5 , there are two possible solutions to this equation; the 
quadrant must be chosen by some other criterion such as continuity. Note the singularity when 
sin 5 = 0, causing the z 3 and z 5 axes to line up. There is no numerically-sound shortcut to finding 
s$i and c0 4 , required below, so that 2 more transcendental function calls are needed. 

Similarly, 5 and 6 can be found by equating A 5 A 6 and Aj 3 W 6 : 



A 5 A fi = 



A[ 3 W 6 = 



■c6 5 c6 6 —c6 s s$ 6 -s$ 5 " 

s9 5 c9 6 —s9 5 s$ 6 c9 s (37) 

-s9 6 -c<? 6 

'W n c9 4 + W 2 lS$4 W^cOi -f W 2 2S04 U>i 3 C0 4 -f 1U 2 3 5 04 

w 3i w 32 w 33 (38) 

.V>uS04 — W21C64 Wi2«^4 — W22CO4 W 13 S04 — W2 3 c8 4 - 

9 b can be found from the 13 and 23 elements of these last matrices and the value of 9 A . 



f**\ 



$9 5 — — tU 13 C0 4 — W 2 3«^4 

c$ 5 =w 33 
By selecting the 31 and 32 elements of these matrices, 



(39) 
(40) 

(41) 



f*S 



s9q = — 1UnS04 + 1021 C04 
C9 6 — — tyi 2 S0 4 -f W22C94 



9r = tan' 



w 



(42) 
(43) 

(44) 



This particular method of computing 6 e has been chosen because it yields s9 6 and c0 6 , and yields 
6 unambiguously. 

4.1. Computational complexity 

The total computational complexity for steps T4 comes to 64 multiplications, 38 additions, and 
10 transcendental function calls, broken down as follows: 

Step 1: 9 multiples and 9 additions. 

Step 2: 15 multiplications, 9 additions, and 5 transcendental function calls. In deriving the additions 
and multiplications, we note that for 6 3 die subexpressions <z| + s\ and 2<z 2 s 4 can be precomputed. 

Step 3: 34 multiplications and 17 additions. Note that only seven elements of 3 W 6 are required, 
namely w n ,w 1 2,wi 3 , w 2 \ , w 2 2, w 23 , w 33 . 

Step 4: 6 multiplications, 3 additions, and 5 transcendental function calls. 
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5. Inverse Kinematic Velocities 

Step 1: Find the wrist linear velocity. The wrist and hand tip linear velocities are related by 

P4 = P7 — W 6 X P7 

Step 2: Find the first three joint velocities. The wrist position is given by 



p 4 = s^o + 02X2 4- S4Z3 



Differentiating, 



p4 = W 2 X 02^2 + W 3 X S4Z3 

When evaluated in link 2 coordinates (Appendix 1), 



'l>4 



-s 4 c0 3 (0 2 + 3 ) 

— #1 Pwx 



(45) 
(46) 
(47) 

(48) 



where 2 p 4 = (AiA 2 ) r i> 4 . Solving, 



•1 — T 



Piz 



Pwx 

2a. ,a 2. 



02 = 



P4</C0 3 — 2 p4 x s6 3 



e. 



a 2 c6 z 

2 P4x — h(s4Ce 3 ) 



(49) 



S4C03 



Step J: F/wrf the angular velocity of the hand relative to the forearm The hand angular 
velocity relative to the forearm, u h , is given by 



(50) 



This is best evaluated in joint 4 coordinates, accomplished as follows: 



3 w 3 = -{e 2 + k) 

jiih + Oi). 

3 W 6 =( A 1 A 2A 3 ) T W6 



V = 3 w 6 - 3 w 3 



4,, —A 73 ,, 



3 Uk x c0 4 + 3 Wfc y s0 4 

3 W hx S0 4 — 3 U> h yC04 



(51) 

(52) 
(53) 

(54) 
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Step 4: Find the last three joint velocities. The hand angular velocity relative to the forearm 
is also given by 



U h = Z 3 4 + *ih + Z5^6 

Expressed in joint 4 coordinates, we find that 



(55) 



4 » — 
Z3 — 


4 y 4 




4 *5 = 


c0 5 

- . 






-06**6 


W = 


04 + 06C05 




L h 





(56) 
(57) 

(58) 



Solving, 



f*s 



■f\ 



05 
0fi 



Wfc* 



505 



04 — Wfc y — 06C05 



5.1. Computation Complexity 



(59) 
(60) 
(61) 



The total for steps 1-4 is 37 multiplications and 25 additions. In arriving at these numbers, 
we presume to have the results of the inverse kinematic position computation available. The 
breakdown is as follows: 

Step I: 6 multiplications and 6 additions. 

Step 2: 15 multiplications and 7 additions. 

Step 3: 14 multiplications and 11 additions. 

Step 4: 2 multiplications and 1 addition. 

6. Inverse Kinematic Accelerations 

Step 1: Find the wrist linear acceleration. This is readily found as 

P4=P 7 -W6 XP7-W 6 X(Wf. XP7) (62) 

Step 2: Find the first three joint accelerations. Differentiating (47), and noting that p w = p 4 , 
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P 4 = y.2 X Pw + W 2 X p 4 — S 4 3 X 3 — S 4 3 U 3 X X 3 

Expressed in joint 2 coordinates, this equation evaluates to (Appendix II) 



-S 4 C0 3 (02 + H) 

0,262 — S 4 S6 3 ($2 + #3) 

— 1 Pwx$l 



% — W 2 X P4 + 



S i 9 3 S0 3 S CJ 3y 

— S 4 3 C0 3 S OJ 3y 

— S 4 3 W3 2 — Pwy8\02. 



Defining the right side as 2 ii 4 , the joint accelerations can now be found. 



(63) 



(64) 



/"""N 



_ U4z 
Pwx 

2 u 4y c0 3 — 2 u 4x s0 3 
a 2 c0 3 

__ 2 U 4x -f S 4 C0 3 '$2 

s 4 c0 3 



(65) 
(66) 
(67) 



Step 3: Find the hand angular acceleration relative to the forearm. By differentiating (50), we 
find that 

<k h = w 6 — w 3 — w 3 X u h (68) 

The angular acceleration w 3 is found in link 3 coordinates from the previous results. 



*& = 


6l$2 






l <k.2 — 


0i 






2 w 2 = 


^2 '& 


I" Ma<*2 


2 W 3 = 


2 w 2 + 2 x 2 3 + 


-010 3 S0 2 




3 w 3 =, 


tf a A 


3 





(69) 



Evaluating in link 4 coordinates, 



3 u> 6 =(A 1 A 2 A 3 ) 7 'w 6 

4 ,-! — A^Vi 3 • 3, . v 3, , \ 

feih. — A 4 i Siie — ^3 — ^3 x &iJ 



(70) 
(71) 
(72) 



(73) 
(74) 
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Kinematic Parameter Multiplications Additions 

Joint Angles 64 38 

Joint Velocities 37 25 

Joint Accelerations 78 57 



Combined Total 



179 



120 



Table 4. Computational complexity for evaluation of the inverse kinematic positions, velocities, and 
accelerations. 

Step 4: Find the last three joint accelerations. Written in terms of the last three angular 
accelerations, the relative hand acceleration is 



W k = Z 3 4 + iJs + Z5#6 + Z3^4 X Zih + fah + Z 4 5 ) X Z 5 6 

When evaluated in joint 4 coordinates, 



(75) 



$4#5 ~~ ^5^6 C ^5 

dAsOs 



-0 6 s6 5 

'hch + 'h 

h 



(76) 



./f> 



The joint accelerations can now be found. 



0« = 



4 u kx - hh + hhcOj 
s6k 



#5 = &hz — 6i6§$0s 

64 = 4 ^hy + hhsd 5 -hc6 b 



(77) 

(78) 
(79) 



6.1. Computational Complexity 

The total is 78 multiplications and 57 additions, using the results of the previous inverse calculations. 
This is constituted as follows: 

Step I: 12 multiplications and 12 additions. Here w 6 x P7 is already known. 

Step 2: 29 multiplications and 17 additions. It is required to compute 2 w 2 here. 

Step 3: 30 multiplications and 23 additions. 

Step 4: 8 multiplications and 5 additions. 

Table 4 summarizes the results for computation of the inverse kinematic positions, velocities, and 
accelerations. 
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link i 
center 




origin 



Figure 5. An illustration of vectors defined in the text, 

7. Dynamics Computation 

In the recursive Newton- Euler dynamics formulation, which is the most efficient one (HoIIerbach 
1980), a kinematics computation precedes application of the Newton-Eulcr equations. Because 
of the simplified kinematics of spherical wrist robots, the kinematic portion of the dynamics 
computation is simplified as well. In addition, the inverse kinematic computations produce some 
of the same quantities as does the inverse dynamics computation. Therefore a combined inverse 
kinematic - inverse dynamics computation would save some operations in the latter computation. 

7.1. Recursive Newton- Euler Dynamics 

The recursive Newton-Euler computation of the dynamics proceeds by a two-step recursive 
procedure (Luh, Walker, and Paul 1980a). First, the angular velocities and accelerations of each 
link are computed along with the linear acceleration of each joint. 



% =Af i - 1 p t _ 1 + »& X 'p* + % X (% X 'p*) 



(80) 
(81) 
(82) 



where these particular equations presume a rotary joint manipulator only. From these results, the 
Newton-Euler equations may be applied to find the net force and torque acting at each link. 



% =% + % X 'r* + % x (% X 'r*) 

% =%% + % X {%%) 
where previously undefined terms are (Figure 5): 



(83) 
(84) 
(85) 



16 



Hollerbach and Sahar Wrist-Paititioned Inverse Kinematics 



'r* is a vector from coordinate system i to the center of mass of link »', 
% is the acceleration of the center of mass of link i, 
% is the net force on link i, 
{ tti is the net torque on link i, and 
'I, is the inertia tensor of link % about its center of mass. 
Secondly, the forces and torques are propagated from the tip to the base. 

%- U =% + A i+1 i+ % i+ i (86) 

! n t _ 1)t =«n, + A t+1 , ' +1 n M +i + (*'p* + ; r*) x % + 'p* x %, i+l (87) 

T i =«'- 1 z i _ 1 -'- l n < _ 1 , i (88) 

where 

%—i,i is the force exerted on link i by link i — 1, 

*n» j.,£ is the moment exerted on link i by link i — 1, and 

Ti is the input torque at joint i. 

Evaluation of these dynamics for a general 6-rotary-joint manipulator requires 688 multiplications 
and 558 additions (a slightly lower figure than given in (Hollerbach 1980)). 

*> 7.2. Dynamics Particularized to a Spherical-Wrist Manipulator 

By taking advantage of the simple kinematic structure of the spherical-wrist, rotary manipulator, 
the dynamic complexity is reduced to 448 multiplications and 361 additions. The breakdown for 
the savings is as follows. 

A,: 116 multiplications and 87 additions are saved. A matrix multiply now takes 4 multiplications 
and 2 additions instead of 8 multiplications and 5 additions because each a; is either 
or ±7r/2. There are 29 applications of the link transformation matrices Aj for the six-joint 
manipulator. 

pV an additional 92 multiplications and 78 additions are saved. This comes about because (i) 
p, = p,-! for i = 1,3,5,6, although the term % = Af ' _1 p',_ 1 must still be evaluated; 
and (ii) p* 2 = z x a 2 , p 4 = Z3S4 simplify some of the cross products. 

Ht—i,;: an additional 32 multiplications and 32 additions saved. As above, this comes about from 
the simple p* vectors when evaluating 'p* x 'fi.i+i. 

w , w : If the assumption of a non-spinning base is made, i.e., y = w = 0, then an additional 
40 multiplications and 37 additions is saved in evaluating l u lt 2 w 2 , 1 &i, 2 w 2 , 1 *ri, and 1 ni. 

Counting the non-spinning base assumption, the dynamic complexity is reduced to 408 multiplications 
and 324 additions, a savings of 280 multiplications and 234 additions. It should be noted that if 
the A, matrices are not provided to the dynamic computation, then an additional 4 multiplications 
are required to form each matrix in the general case. This would be unnecessary for the particular 

manipulator considered here. 
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7.3. Inverse Kinematics Considered with Inverse Dynamics 

In computing the inverse kinematics, there is an additional modest savings in the inverse dynamics 
of 40 multiplications and 28 additions, bringing the total dynamics complexity down to 368 
multiplications and 296 additions. This results from the precomputation of the following quantities. 

1 w 1 , 2 w 2 , 3 w 3 : 6 multiplications and 3 additions are saved. 

^i, 2 ^, 3 ^: 10 multiplications and 7 additions are saved. 

4 p 4 : 4 multiplications and 4 additions are saved. Since 2 p 4 is known, then 

3 p 4 = Aj 2 p 4 , 4 p 4 = Aj 3 p 4 

requires 8 multiplications and 4 additions for evaluation instead of 12 multiplications and 8 
additions from (82). 

2 jp 2 : 8 multiplications and 6 additions are saved. It is not necessary to compute p 2 because 2 r 2 
can be computed without it and p 4 is known. From (82) and (83), 

2 r 2 = Al% + 2 w 2 x ( 2 p 2 + 2 r 2 ) + 2 w 2 x ( 2 u> 2 X ( 2 p 2 + 2 r* 2 )) (89) 

3 p 3 : 4 multiplications and 2 additions saved. Similarly, it is not necessary to compute p 3 because 
3 r 3 can be computed from 3 p 4 . 

f~\ : 3 P 3 = 3 P 4 — 3 w 3 X s 4 3 z 3 - 3 w 3 x ( 3 w 3 X s 4 3 zs) (90) 

3 r 3 = 3 P 4 + 3 ^3 X ( 3 r* - s 4 3 z 3 ) + 3 w 3 X ( 3 w 3 X ( 3 4 - s 4 3 z 3 )) (91) 

where 3 r 3 — s 4 3 z 3 is fixed and is precomputed. 

4 w 4 , 5 w 5 , e w 6 : 3 multiplications and 2 additions saved. This results from calculating 6 w 6 directly 
from w 6 and the components of 4 u h separately. 

6 w 6 =Wju> 6 (92) 

^e-Ae 6 ^ (93) 

4 w 6 =A 5 5 w 6 (94) 

4 w 3 =A[ 3 a; 3 (95) 

4 w fc =V 6 -V 3 (96) 

*0U = 4 w 3 + 4 y 4 <? 4 (97) 

5 w 5 = 5 w 6 — 5 z 5 6 (98) 

Ordinarily, 24 multiplications and 19 additions would have been required to calculate these 
quantities. 
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6 W6 


=W^ 6 




5 w 6 


=A 6 6 w 6 




4 ^ 6 


=A 5 5 u> 6 




4 W 3 


-Al 3 to 




^ 


4 • 
— faj 6 — 


4 w 3 — 4 w 3 X 4 w^ 


W4 


= 4 &3 + 


4 y 4 4 + 4 y 3 x 4 y 4 ^ 


5 ^5 


= 5 w 6 - 


5 Z 5 ^6 - 5 W 5 X 5 Z 5 ^6 
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4 w 4 , 5 w 5) 6 w 6 : 5 multiplications and 4 additions saved. Similarly, some savings can be accomplished 
by computing 6 w 6 from w 6 and die components of 4 u h separately. 



(99) 
(100) 
(101) 
(102) 
(103) 
(104) 
(105) 

Ordinarily, 36 multiplications and 31 additions are required for these quantities. 

7.4. Simplified Inertial Parameters 

No presumptions were made above about the inertial parameters of the manipulator, namely the 
center of gravity, the principal inertias, and the orientation of the principal axes of inertia for 
each link. While the kinematic structure of manipulators is deliberate, the inertial parameters are 
seldom a factor in design. Therefore a simplifying set of inertial parameters cannot be assumed a 
priori. Paul (1981) considered one way in which the dynamics might be simplified if a particular 
set were assumed. In the present study, if the center of gravity and the principal axes lined up 
with the internal link coordinate system, an additional simplification of 174 multiplications and 
168 additions in the dynamics would result 

%: 60 multiplications and 54 additions are saved. If ; 'r* lies along a coordinate axis, then each 
% requires only 8 multiplications and 6 additions for evaluation. 

%—!,,: 24 multiplications and 24 additions are saved. The term (*p* + 'r*) x { U now requires 2 
multiplications and 2 additions for its formation and its addition, where 'p* + e r* lies along 
an axis. 

•n^: 90 multiplications and 90 additions are saved. If % is aligned with the coordinate axes, then 
it is diagonal. Three additions are saved in the i w i X '1,'u^ term because the principal inertia 
differences can be precomputed. 

If the simplified inertial parameters were considered along with all the other computational 
savings, then the dynamics would require only 194 multiplications and 138 additions. Table 5 
summarizes the dynamics complexity for the various kinematic and dynamic conditions that have 
been considered above. It appears that the dynamic equations are even more approachable from 
a computational standpoint than had been considered heretofore (Hollerbach 1980). Under the 
best conditions, an exact evaluation of the dynamics has roughly the same complexity as a full 
evaluation of the inverse kinematics (Table 4). 
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jf*% 



688 


558 


408 


324 


368 


296 


194 


138 



Condition Multiplications Additions 

General Rotary Manipulator 

Spherical Wrist Manipulator 

Precomputed Inverse Kinematics 

Simplified Incrtial Parameters 

Table 5. Dynamics computation complexity for a 6-dof manipulator under various conditions. 

8. Discussion and Summary 

An algorithm for calculation of the inverse kinematic accelerations has been presented, which 
is the most efficient one to date. Based on a method developed by Fcatherstone (1983), the 
algorithm directly takes advantage of the structure of spherical-wrist manipulators to decompose 
the 6-dof inverse problem into two 3-dof inverse problems through a 4-step procedure. The 
resultant equations for the first 3 joints, but not the last three, are particular to a given manipulator 
structure, but the technique is easily extended to other structures. One could envision a catalog of 
equations for the most common manipulator configurations. 

Spherical wrists are the most important case for current 6-dof robots and are becoming 
standard. An additional simplifying kinematic criterion, namely that neighboring joint axes are 
oriented parallel or orthogonal to each other, is almost always followed in manipulator design and 
aids the solution to the two 3-dof kinematic problems. 

/-■^ These two kinematic criteria have a simplifying effect on the dynamic equations as well. 

Since in the recursive Newton-Euler equations (Luh, Walker, and Paul 1980a) the computations 
are carried out in internal link coordinates, the transformations between neighboring links are 
simplified. Angular velocities and accelerations as well as linear accelerations of die link origins 
are calculated more simply, even as they are in the inverse kinematics computation. Because the 
inverse kinematic acceleration algorithm produces some of these last-mentioned vectors as well, 
then a combined inverse kinematic acceleration/inverse dynamics computation results in some 
savings for die inverse dynamics calculation. 

The implication of these algorithms is that the computational requirement for either the 
inverse kinematics or die inverse dynamics is low enough to warrant easy real-time implementation. 
Recent control algorithms based on derived hand acceleration (Luh, Walker, and Paul 1980b, 
Freund 1982, Hogan and Cotter 1982) are made computationally feasible, making these algorithms 
more competitive with schemes which close force loops around the hand to avoid die inverse 
kinematic problem (Khatib 1980). The recursive Newton-Euler formulation is made even more 
efficient; if simplifying inertial parameters are assumed, the dynamics complexity roughly equals 
the inverse kinematics complexity. It would seem diat a general Cartesian trajectory control ability, 
involving full inverse kinematics and dynamics as well as sophisticated hand-based control laws, 
is within reach. 
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Appendix I 

The wrist velocity is given by 

where 
Substituting above, 



p 4 = w 2 x o 2 x 2 + w 3 x S4Z3 



w 3 = w 2 + Z 2 3 



p 4 = w 2 X (a 2 x 2 + S4Z3) -f- 3 z 2 X Z3S4 
Noting that p w = a 2 x 2 + s 4 z 3 and x 3 = — z 2 x z 3 , 



where 



p 4 = w 2 x p w — s 4 ^ 3 x 3 



W 2 = Zq^i -f- Zi# 2 



(106) 



(107) 



(108) 



(109) 



(110) 



The wrist velocity is best evaluated in joint 2 coordinates, a reflection of the regular kinematic 
structure of the first 3 joints. Since the rotation axes are either perpendicular or parallel to each 
other, then a rotation axis in one coordinate system will also be a major axis in the next coordinate 
system. Link 2 coordinates are the most convenient because they are situated in the middle of the 
coordinate systems. Starting first with the angular velocity 2 w 2 , 



/"\ 







'S$ 2 


2, _2 V _ 

zo — yi — 


c6 2 




L 


2 Zj =t 2 Z 2 




Bis6 2 




w 2 = 


e 1 c9 2 





The vector p w in link 2 coordinates is 



P™ = 



a 2 — S4S63 
S4C03 





Thus the cross product term in (109) evaluates to 
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2 w 2 X 2 p w 



—02 Pwy 
h 2 Pwx 

h{ 2 Pwys0 2 — 2 p wx c6 2 ), 



-e 2 { Si c$ z ) 

9 2 {a 2 — s 4 s0 3 ) 

. —01 1 Pwx . 



where it is noticed that V^i — —{ 2 Pw y s9 2 — 2 p™xC0 2 ). Finally, 



2 p 4 = 2 u> 2 x 2 p w — s 4 3 2 x 3 

2 X 3 =( C 3 ,S0 3 ,O) 



Collecting all terms, (79) becomes 



<P 4 



-S 4 C0 3 (0 2 + h) 
a 2 b 2 - s 4 s9 3 (9 2 + h) 

— 01 Pwx 



(115) 



(116) 
(117) 



(118) 
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Appendix II 

The wrist acceleration in link 2 coordinates is 

2 p 4 = 2 w 2 X 2 p w + 2 w 2 X 2 p 4 — s 4 03 2 x 3 — s 4 3 2 w 3 X 2 x 3 
Starting with the first term on the right, 



W 2 =(0102,01,02) 

P-u; === ( Pwxt Pwy i "j 

— 02 Pwy 

02 X Pwx 

.01 02 Pwy — 01 Pwx. 

h^PwxSh — 1 p wy c9 2 ) 
6 2 { l p wx c9 2 + 1 PwyS0 2 ) 
. '8lh l Pwy -—01 1 Pwx . 



^2 X J P 



This expression can be further simplified by noting that 

Pwx s ^2 — Pwy c 02 = — S 4 C0 3 
PwxC0 2 + PwyS0 2 =a 2 — SiS$ 3 
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(123) 
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The last term on the right is evaluated as follows: 
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3 w 3 X 3 X 3 



A 3 ( 3 w 3 X 3 x 3 ) 



■ 
3 w 32 

' s9 3 3 w 3s/ 
— c9 3 s u 3y 

— 3 w 32 



(126) 



(127) 



Collecting all unknowns on one side, 



—s 4 c0 3 9 2 — s 4 c$ 3 9 3 

(o 2 — S 4 S0 3 )0 2 — S 4 S$ 3 9 3 
— 1 PwJl 



'P4 



2 W 2 X 2 P 4 + 



s 4 6 3 s6 3 3 u) 3 y 
—s 4 3 c9 3 s u 3y 

-Sih 3 w 30 — 1 p wy $ 1 e 2 , 



(128) 
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